#include <PS2X_lib.h>

#define CLK  7
#define CMD  5
#define CS   6
#define DAT  4
PS2X ps2x;

//right now, the library does NOT support hot-pluggable controllers, meaning
//you must always either restart your Arduino after you connect the controller,
//or call config_gamepad(pins) again after connecting the controller.

int error = 0;
byte type = 0;
byte vibrate = 0;
int count = 10;

void setup()
{
  Serial.begin(57600);

  while (count > 0)
  {
    count--;
    error = ps2x.config_gamepad(CLK, CMD, CS, DAT, true, true); //GamePad(clock, command, attention, data, Pressures?, Rumble?)
    if (error == 0)
    {
      Serial.println("Found Controller, configured successful");
      Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
      Serial.println("holding L1 or R1 will print out the analog stick values.");
      Serial.println("Go to www.billporter.info for updates and to report bugs.");
      break;
    }
    else if (error == 1)
      Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
    else if (error == 2)
      Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
    else if (error == 3)
      Serial.println("Controller refusing to enter Pressures mode, may not support it. ");

    delay(100);
  }

  type = ps2x.readType();
  switch (type)
  {
  case 0:
    Serial.println("Unknown Controller type");
    break;
  case 1:
    Serial.println("DualShock Controller Found");
    break;
  case 2:
    Serial.println("GuitarHero Controller Found");
    break;
  }
}

void loop()
{
  /* You must Read Gamepad to get new values
       Read GamePad and set vibration values
       ps2x.read_gamepad(small motor on/off, larger motor strenght from 0-255)
       if you don't enable the rumble, use ps2x.read_gamepad(); with no values
       
       you should call this at least once a second
       */

  if (error == 1)
    return;

  if (type == 2)
  {

    ps2x.read_gamepad(); //read controller

    if (ps2x.ButtonPressed(GREEN_FRET))
      Serial.println("Green Fret Pressed");
    if (ps2x.ButtonPressed(RED_FRET))
      Serial.println("Red Fret Pressed");
    if (ps2x.ButtonPressed(YELLOW_FRET))
      Serial.println("Yellow Fret Pressed");
    if (ps2x.ButtonPressed(BLUE_FRET))
      Serial.println("Blue Fret Pressed");
    if (ps2x.ButtonPressed(ORANGE_FRET))
      Serial.println("Orange Fret Pressed");

    if (ps2x.ButtonPressed(STAR_POWER))
      Serial.println("Star Power Command");

    if (ps2x.Button(UP_STRUM)) //will be TRUE as long as button is pressed
      Serial.println("Up Strum");
    if (ps2x.Button(DOWN_STRUM))
      Serial.println("DOWN Strum");

    if (ps2x.Button(PSB_START)) //will be TRUE as long as button is pressed
      Serial.println("Start is being held");
    if (ps2x.Button(PSB_SELECT))
      Serial.println("Select is being held");

    if (ps2x.Button(ORANGE_FRET)) // print stick value IF TRUE
    {
      Serial.print("Wammy Bar Position:");
      Serial.println(ps2x.Analog(WHAMMY_BAR), DEC);
    }
  }

  else
  {                                    //DualShock Controller
    ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed

    if(ps2x.Button(PSB_START)) //will be TRUE as long as button is pressed
      Serial.println("Start is being held");
    else if (ps2x.Button(PSB_SELECT))
      Serial.println("Select is being held");
    else if (ps2x.Button(PSB_SELECT))
    {
      Serial.println("Select is being held");
    }
    else if (ps2x.Button(PSB_PAD_UP))
    { //will be TRUE as long as button is pressed
      Serial.print("Up held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
    }
    else if (ps2x.Button(PSB_PAD_RIGHT))
    {
      Serial.print("Right held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC);
    }
    else if (ps2x.Button(PSB_PAD_LEFT))
    {
      Serial.print("LEFT held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);
    }
    else if (ps2x.Button(PSB_PAD_DOWN))
    {
      Serial.print("DOWN held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
    }
    else if (ps2x.Button(PSB_RED))
    {
      Serial.print("Circle held this hard: ");
      Serial.println(ps2x.Analog(PSB_RED), DEC);
    }
    else if (ps2x.Button(PSB_PINK))
    {
      Serial.print("Square held this hard: ");
      Serial.println(ps2x.Analog(PSB_PINK), DEC);
    }
    else if (ps2x.Button(PSB_BLUE))
    {
      Serial.print("X held this hard: ");
      Serial.println(ps2x.Analog(PSB_BLUE), DEC);
    }
    else if (ps2x.Button(PSB_GREEN))
    {
      Serial.print("Triangle held this hard: ");
      Serial.println(ps2x.Analog(PSB_GREEN), DEC);
    }
    else if (ps2x.Button(PSB_L3))
    {
      Serial.print("L3 held this hard: ");
      Serial.println(ps2x.Analog(PSB_L3), DEC);
    }
    else if (ps2x.Button(PSB_L2))
    {
      Serial.print("L2 held this hard: ");
      Serial.println(ps2x.Analog(PSB_L2), DEC);
    }
    else if (ps2x.Button(PSB_L1))
    {
      Serial.print("L1 held this hard: ");
      Serial.println(ps2x.Analog(PSB_L1), DEC);
    }
    else if (ps2x.Button(PSB_R3))
    {
      Serial.print("R3 held this hard: ");
      Serial.println(ps2x.Analog(PSB_R3), DEC);
    }
    else if (ps2x.Button(PSB_R2))
    {
      Serial.print("R2 held this hard: ");
      Serial.println(ps2x.Analog(PSB_R2), DEC);
    }
    else if (ps2x.Button(PSB_R1))
    {
      Serial.print("R1 held this hard: ");
      Serial.println(ps2x.Analog(PSB_R1), DEC);
    }
  }

  delay(50);
}
